{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "import glob\n",
    "from tqdm import tqdm\n",
    "from math import degrees as dg\n",
    "from math import sqrt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "******************使用cv2.findEssentialMat计算本质矩阵,从而获得两相机相关位姿(R,T)******************\n",
    "\"\"\"\n",
    "# 棋盘格规格\n",
    "chessboard_size = (7,7)\n",
    "# 相机内参和外参路径\n",
    "cParas_path = r'C:/Users/HuangSX/PycharmProjects/cv2/'\n",
    "# 图片的绝对路径\n",
    "pic_path = r'C:/Users/HuangSX/Desktop/OpenCV/data/'\n",
    "# 加载相机标定的内参数、外参数矩阵\n",
    "with np.load(cParas_path + 'C.npz') as X:\n",
    "    mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 使用畸变矩阵去除图片畸变，并且适度裁剪(图片优化处理)\n",
    "def get_optimal_pic(path, pic_name, mtx, dist):\n",
    "    print(path + str(pic_name))\n",
    "    img2 = cv2.imread(path + str(pic_name))\n",
    "    print(\"orgininal img_point array shape:\",img2.shape)\n",
    "\n",
    "    h,  w = img2.shape[:2]\n",
    "    print(\"pic's hight, weight: %f,  %f\"%(h, w))\n",
    "\n",
    "    new_mtx, roi = cv2.getOptimalNewCameraMatrix(\n",
    "        mtx, dist, (w, h), 1, (w, h))  # 自由比例参数\n",
    "\n",
    "    new_pic = cv2.undistort(img2, mtx, dist, None, new_mtx)\n",
    "    \n",
    "    # 根据前面ROI区域裁剪图片\n",
    "    x,y,w,h = roi\n",
    "    new_pic = new_pic[y:y+h, x:x+w]\n",
    "    pic_new_name = str(\"opt_\") + str(pic_name)\n",
    "    new_path_name = str(path) + pic_new_name\n",
    "    cv2.imwrite(new_path_name, new_pic)\n",
    "    print(\"##################\"+str(pic_new_name)+\"##################\")\n",
    "    print(\"get new optimal ROI picture\")\n",
    "    return pic_new_name"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 寻找图片角点，并且标注角点位置\n",
    "def get_corners_mat(path, pic_name, optal_or_not):\n",
    "    img = cv2.imread(str(path) + str(pic_name))\n",
    "    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "    \n",
    "    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)\n",
    "    \n",
    "    if ret == True:\n",
    "        # 提高像素位置的精度\n",
    "        corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)\n",
    "        # 标注角点\n",
    "        img = cv2.drawChessboardCorners(img, chessboard_size, corners2, ret)\n",
    "              \n",
    "        count = 0\n",
    "        print(\"##################\"+\"corner_\" + str(pic_name)+\"##################\")\n",
    "        for corner in corners:\n",
    "            x_label,y_label = corner[0]\n",
    "            #print(\"corner_num:\"+str(count)+\" /location:\"+str(corner))\n",
    "            cv2.putText(img, '(%s)' % (count), (x_label,y_label), cv2.FONT_HERSHEY_COMPLEX, \n",
    "                        0.5, (255, 255, 255), 1)\n",
    "            count = count + 1\n",
    "            \n",
    "        if optal_or_not:\n",
    "            cv2.imwrite(path + \"corner_\" + str(pic_name), img) \n",
    "        else:\n",
    "            cv2.imwrite(path + \"orig_\" + \"corner_\" + str(pic_name), img)\n",
    "        cv2.destroyAllWindows()\n",
    "        print(\"finish find chess board\")\n",
    "    else:\n",
    "        print(\"failed to find chess board\")\n",
    "        return None\n",
    "    \n",
    "    return corners"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "C:/Users/HuangSX/Desktop/OpenCV/data/leftTest.jpg\n",
      "orgininal img_point array shape: (4032, 3024, 3)\n",
      "pic's hight, weight: 4032.000000,  3024.000000\n",
      "##################opt_leftTest.jpg##################\n",
      "get new optimal ROI picture\n",
      "C:/Users/HuangSX/Desktop/OpenCV/data/rightTest.jpg\n",
      "orgininal img_point array shape: (4032, 3024, 3)\n",
      "pic's hight, weight: 4032.000000,  3024.000000\n",
      "##################opt_rightTest.jpg##################\n",
      "get new optimal ROI picture\n"
     ]
    }
   ],
   "source": [
    "# 图片去畸变和适度裁剪(图片优化处理)\n",
    "left_pic_name = get_optimal_pic(pic_path,\"leftTest.jpg\", mtx, dist)\n",
    "right_pic_name = get_optimal_pic(pic_path,\"rightTest.jpg\", mtx, dist)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 读取原始图片左右相机图片\n",
    "# org_corners_rt = get_corners_mat(pic_path,\"left.jpg\", False)\n",
    "# org_corners_lf = get_corners_mat(pic_path,\"right.jpg\", False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "##################corner_opt_leftTest.jpg##################\n",
      "finish find chess board\n",
      "##################corner_opt_rightTest.jpg##################\n",
      "finish find chess board\n"
     ]
    }
   ],
   "source": [
    "# 读取去畸变之后、左右相机图片\n",
    "corners_lf = get_corners_mat(pic_path, left_pic_name, True)\n",
    "corners_rt = get_corners_mat(pic_path, right_pic_name, True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Essential Matrix:  [[-0.04279498 -0.68036227 -0.1224968 ]\n",
      " [ 0.62098301 -0.01233895  0.18233634]\n",
      " [ 0.27019949 -0.158959    0.05455171]]\n"
     ]
    }
   ],
   "source": [
    "# 带入参数寻找本质矩阵（使用原始图片角点）\n",
    "# E, mask = cv2.findEssentialMat(org_corners_lf, org_corners_rt,\n",
    "#                                cameraMatrix=mtx, method=cv2.RANSAC,\n",
    "#                                threshold=1, prob=0.999\n",
    "#                                )\n",
    "# print('E: ', E)\n",
    "\n",
    "# 带入参数寻找本质矩阵（使用去畸变之后的角点）\n",
    "E, mask = cv2.findEssentialMat(corners_lf, corners_rt,\n",
    "                               cameraMatrix=mtx, method=cv2.RANSAC,\n",
    "                               threshold=1, prob=0.999)\n",
    "print('Essential Matrix: ', E)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "retval2: 24\n",
      "####################\n",
      "rotation matrix: [[-0.88141912  0.13683685 -0.45207965]\n",
      " [ 0.13606536 -0.84298542 -0.52044385]\n",
      " [-0.45231245 -0.52024154  0.72440471]]\n",
      "####################\n",
      "trasform matrix: [[-0.20134697]\n",
      " [-0.40244581]\n",
      " [ 0.89302675]]\n",
      "####################\n"
     ]
    }
   ],
   "source": [
    "# 本质矩阵分解成R,T矩阵\n",
    "# retval2, R, t, mask = cv2.recoverPose(E, org_corners_lf, org_corners_rt, mtx)\n",
    "\n",
    "retval2, R, t, mask = cv2.recoverPose(E, corners_lf, corners_rt, mtx)\n",
    "\n",
    "print(\"retval2:\", retval2)\n",
    "print(\"##\"*10)\n",
    "print(\"rotation matrix:\", R)\n",
    "print(\"##\"*10)\n",
    "print(\"trasform matrix:\", t)\n",
    "print(\"##\"*10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sita 0 is -50.501596\n",
      "sita 1 is 7.840174\n",
      "sita 2 is -25.902256\n",
      "T's  value is  0.9999999999999999\n"
     ]
    }
   ],
   "source": [
    "dstR, _ = cv2.Rodrigues(R)\n",
    "\n",
    "sita = []\n",
    "for i in range(3):\n",
    "    sita = dg(R[0][i])\n",
    "    print(\"sita %d is %f\"%(i, sita))\n",
    "    \n",
    "value = sqrt(t[0][0]**2 + t[1][0]**2 + t[2][0]**2)\n",
    "print(\"T's  value is \",value)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "****************使用cv2.stereoCalibrate获取两相机相对位置(R,T)****************\n",
    "\"\"\"\n",
    "# termination criteria\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "chessboard_size = (7, 7)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 世界坐标系下的3D坐标\n",
    "objp = np.zeros((np.prod(chessboard_size), 3), dtype=np.float32)\n",
    "objp[:, :2] = np.mgrid[0:chessboard_size[0],\n",
    "                       0:chessboard_size[1]].T.reshape(-1, 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Arrays to store object points and image points from all the images.\n",
    "# 物体的3D坐标点集\n",
    "objpoints = []\n",
    "# 左相机拍摄的图片的点集\n",
    "imgpointsL = []  \n",
    "# 右相机拍摄的图片的点集\n",
    "imgpointsR = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 左相机拍摄到的图片处理\n",
    "images = glob.glob(r'C:/Users/HuangSX/Desktop/OpenCV/data/leftTest.jpg')\n",
    "# 3D坐标点集\n",
    "objpoints.append(objp)\n",
    "\n",
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    grayL = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "\n",
    "    # Find the chess board corners\n",
    "    ret, cornersL = cv2.findChessboardCorners(grayL, chessboard_size, None)\n",
    "    # If found, add object points, image points (after refining them)\n",
    "    if ret == True:        \n",
    "        # 图片上的角点集\n",
    "        cornersL2 = cv2.cornerSubPix(grayL, cornersL, (11, 11), (-1, -1), criteria)\n",
    "        imgpointsL.append(cornersL2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 右相机拍摄到的图片处理\n",
    "images = glob.glob(r'C:/Users/HuangSX/Desktop/OpenCV/data/rightTest.jpg')\n",
    "\n",
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    grayR = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)\n",
    "\n",
    "    # Find the chess board corners\n",
    "    ret, cornersR = cv2.findChessboardCorners(grayR, chessboard_size, None)\n",
    "\n",
    "    # If found, add object points, image points (after refining them)\n",
    "    if ret == True:        \n",
    "        # 图片上的角点集\n",
    "        cornersR2 = cv2.cornerSubPix(grayR, cornersR, (11, 11), (-1, -1), criteria)\n",
    "        imgpointsR.append(cornersR2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Essential matrix E: [[ 0.64137275 -1.9210683   3.3819229 ]\n",
      " [ 2.54417251 -0.58502009  4.93447397]\n",
      " [ 3.73514953  5.28501562 -0.16174696]]\n",
      "##########\n",
      "rotation matrix: [[-0.98099143  0.0132959   0.19359504]\n",
      " [-0.03334877 -0.99435832 -0.10069454]\n",
      " [ 0.19116402 -0.10523664  0.97590039]]\n",
      "####################\n",
      "trasform matrix: [[-5.36347485]\n",
      " [ 3.62519403]\n",
      " [-1.54830009]]\n",
      "####################\n"
     ]
    }
   ],
   "source": [
    "retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R1, T1, E1, F1 = \\\n",
    "cv2.stereoCalibrate(objpoints, imgpointsL, imgpointsR, mtx, dist,mtx, dist, (2448, 3264))\n",
    "\n",
    "print(\"Essential matrix E:\", E1)\n",
    "print(\"#\"*10)\n",
    "print(\"rotation matrix:\", R1)\n",
    "print(\"##\"*10)\n",
    "print(\"trasform matrix:\", T1)\n",
    "print(\"##\"*10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "sita 0 is -56.206668\n",
      "sita 1 is 0.761799\n",
      "sita 2 is 11.092179\n"
     ]
    }
   ],
   "source": [
    "dstR1, _ = cv2.Rodrigues(R1)\n",
    "\n",
    "sita1 = []\n",
    "for i in range(3):\n",
    "    sita1 = dg(R1[0][i])\n",
    "    print(\"sita %d is %f\"%(i, sita1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
